#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright:  Jihua Lab 2021.
@File    :   SEA.py    
@Author :   HaoWANG, Foshan，China
@Contact :   wanghao@jihualab.com
@License :   JHL ( not BSD)

@Description:  


@Create Time   :   2022/6/13 0013  15:49
@Modify Time      @Author    @Version    @Desciption
------------      -------    --------    -----------
2022/6/13 0013 15:49   wanghao      1.1.0         None
'''

# import lib

import time
import ADS1263
import queue


class SEAJointServoStateInfo:
    def __init__(self, sea_servo_list, joint_num=2):
        # left and right shoulder pitch joint, SEA DOF=2

        self.sea_servo_list = sea_servo_list
        self.joint_num = joint_num
        self._sea_servo_angle_balanced = [0 for itm in range(0, joint_num)]
        self.sea_servo_angle_current = [0 for itm in range(0, joint_num)]
        self.sea_servo_angle_relative = [0 for itm in range(0, joint_num)]

    def generate_sea_state_vec(self):
        # key: servo id, Value: servo current angle
        sea_state_vec = []
        for servo_id in self.sea_servo_list:
            key = servo_id
            val_curr = self.sea_servo_angle_current[self.sea_servo_list.index(servo_id)]
            val_rel = self.sea_servo_angle_relative[self.sea_servo_list.index(servo_id)]
            sea_state = [key, val_curr,
                         val_rel]  # get the current angle and relative angle of SEA joint, the key is the servo id index
            sea_state_vec.append(sea_state)
        return sea_state_vec  # return the generated sea joint state information, which is a list

    def get_balanced_angle(self, servo_id):
        # get the sea joint balanced refrence and joint servo id
        if servo_id in self.sea_servo_list:
            return self._sea_servo_angle_balanced[self.sea_servo_list.index(servo_id)]
        else:
            return False

    def print_balanced_angle(self):
        return self._sea_servo_angle_balanced

    def set_balanced_angle(self, servo_id, cst_angle):
        # set the private value of the angle balanced angle value, which is a static float type value
        # init with servo id and cst_angle
        if servo_id in self.sea_servo_list:
            self._sea_servo_angle_balanced[self.sea_servo_list.index(servo_id)] = cst_angle
            return True
        else:
            print("Error: servo_id NOT in sea_servo_list! ")
            return False

    def set_current_angle(self, current_angle_list):
        # read and set the sea joint current angle with a angle value list
        if current_angle_list.size() == self.joint_num:
            self.sea_servo_angle_current = current_angle_list
            return True
        else:
            return False

    def set_relative_angle(self):
        if self.sea_servo_angle_current:
            for idx in range(0, self.joint_num):
                self.sea_servo_angle_relative[idx] = self.sea_servo_angle_current[idx] - self._sea_servo_angle_balanced[
                    idx]
            return True
        else:
            return False


#####
# class SEA modual
class SEA:

    def __init__(self, servo_list):
        self.servo_list = servo_list
        self.joint_num = 2  # default DOF = 2, shoulder pitch
        self.sea_state_list = []
        self.sea_joint_force = []
        self.sea_state_obj = SEAJointServoStateInfo(sea_servo_list=self.servo_list)
        self._has_init_balanced_angle = False
        self.is_debug = True
        self.SEA_Balanced_Angle = []
        self.current_angle = []
        self.frequency = 20  # Hz
        self.REF = 5.20  # Modify according to actual voltage
        # external AVDD and AVSS(Default), or internal 2.5V

        # self.servo_list = [0, 1, 4, 5]  # 0,1 left arm ;4,5 right arm shoulder joint yaw and pitch, respectively

    def init_balanced_angle(self, current_angle):
        print("Preparing for Init the SEA Joint Balanced Position Parameters, Please Don`t Move.")
        if not self._has_init_balanced_angle:
            self.read_all_sea_angle(current_angle_list=current_angle)
            for servo_id in self.servo_list:
                self.sea_state_obj.set_balanced_angle(servo_id,
                                                      self.sea_state_obj.sea_servo_angle_current[
                                                          self.servo_list.index(servo_id)])
            if self.SEA_Balanced_Angle != [] and self.is_debug:
                self.SEA_Balanced_Angle = self.sea_state_obj.print_balanced_angle()
                print(" --- SEA_Balanced_Angle init processes is Completed! : {} ---".format(self.SEA_Balanced_Angle))

            self._has_init_balanced_angle = True  # set init flag
            return self.SEA_Balanced_Angle  # return the balanced angle info

    def read_all_sea_angle(self, current_angle_list):
        if current_angle_list != []:  # empty check
            self.sea_state_obj.set_current_angle(current_angle_list=current_angle_list)
        else:
            print("Error：the current_angle_list is empty! ")

    def update_all_sea_state_vec(self, current_angle):
        # after read the current angle, set the relative angle
        sea_state_vector = []
        if self._has_init_balanced_angle:
            self.read_all_sea_angle(current_angle_list=current_angle)
            if self.sea_state_obj.set_relative_angle() == True:
                # generate the joint angle state vector info
                sea_state_vector = self.sea_state_obj.generate_sea_state_vec()

        return sea_state_vector

    def read_sea_angle(self, servo_id):

        current_angle_buffer = [0, 0]
        if self.current_angle != []:
            self.current_angle.clear()  # clear the current angle buffer of a servo
        else:
            try:
                ADC = ADS1263.ADS1263()
                if (ADC.ADS1263_init_ADC1('ADS1263_7200SPS') == -1):
                    exit()
                ADC.ADS1263_SetMode(0)

                # ADC.ADS1263_DAC_Test(1, 1)      # Open IN6
                # ADC.ADS1263_DAC_Test(0, 1)      # Open IN7

                if (ADC.ADS1263_init_ADC2('ADS1263_ADC2_400SPS') == -1):
                    exit()
                ADC_Value = ADC.ADS1263_GetAll_ADC2()  # get ADC2 value
                for i in range(0, self.joint_num):
                    if (ADC_Value[i] >> 23 == 1):
                        val = self.REF * 2 - ADC_Value[i] * self.REF / 0x800000
                        print("ADC2 IN%d = -%lf" % (i, (self.REF * 2 - ADC_Value[i] * self.REF / 0x800000)))
                        # print("     AVG a : ADC2 IN%d = -%lf" % (i, avg))
                    else:
                        val = ADC_Value[i] * self.REF / 0x7fffff
                        # avg = moving_average_a.next(val)
                        print("ADC2 IN%d = %lf" % (i, (ADC_Value[i] * self.REF / 0x7fffff)))  # 24bit
                        # print("     AVG b : ADC2 IN%d = %lf" % (i, avg))
                    angle = (360.0 * val) / self.REF  # degree of sea

                    current_angle_buffer[i] = angle
                    print("SEA ANGLE : " + format(angle, '.3f'))
                # time.sleep(1 / self.frequency)
                ADC.ADS1263_Exit()

            except IOError as e:
                print(e)

            except KeyboardInterrupt:
                print("ctrl + c:")
                print("Program end")
                ADC.ADS1263_Exit()
                exit()


    def read_sea_force(self, servo_id):
        pass


class MovingAverage:
    def __init__(self, size: int):
        self.que = queue.Queue(size)

    def next(self, val: float):
        if (self.que.full()):
            self.que.get()  # get the queue item value
        self.que.put(val)
        return sum(self.que.queue) / self.que.qsize()  # calculate the average voltage value
